#include "pcpt_comm/pcpt_comm_filter.h"

/**************************************************************************************
功能描述: 体素滤波
修改记录:
**************************************************************************************/
void POINT_CLOUD_FILTER_C::VoxelGridFilter(MyCloud::Ptr pCloud, float leafSize) const
{
    VoxelGridFilter(pCloud, leafSize, leafSize, leafSize);
}

void POINT_CLOUD_FILTER_C::VoxelGridFilter(MyCloud::Ptr pCloud, float length, float width, float height) const
{
    if (0 == pCloud->size())
        return;
    
    pcl::VoxelGrid<MyPoint> filter;
    MyCloud::Ptr pCloudOut(new MyCloud);
    
    filter.setInputCloud(pCloud);
    filter.setLeafSize(length, width, height);
    filter.filter(*pCloudOut);

    if (pCloud->size() != pCloudOut->size())
        *pCloud = *pCloudOut;
}

void POINT_CLOUD_FILTER_C::VoxelGridFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud, float leafSize) const
{
    VoxelGridFilter(pCloud, leafSize, leafSize, leafSize);
}

void POINT_CLOUD_FILTER_C::VoxelGridFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud, float length, float width, float height) const
{
    if (0 == pCloud->size())
        return;
    
    pcl::VoxelGrid<pcl::PointXYZ> filter;
    pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudOut(new pcl::PointCloud<pcl::PointXYZ>);
    
    filter.setInputCloud(pCloud);
    filter.setLeafSize(length, width, height);
    filter.filter(*pCloudOut);

    if (pCloud->size() != pCloudOut->size())
        *pCloud = *pCloudOut;
}

/**************************************************************************************
功能描述: 条件滤波
修改记录:
**************************************************************************************/
void POINT_CLOUD_FILTER_C::ConditionFilter(MyCloud::Ptr pCloud, const CUBOID_STRU &stCuboid) const
{
    if (0 == pCloud->size())
        return;

    MyCloud::Ptr pCloudOut(new MyCloud);
    
#if 1    
    pcl::CropBox<MyPoint> clipper;
    clipper.setMin(Eigen::Vector4f(stCuboid.minX, stCuboid.minY, stCuboid.minZ, 1.0));
    clipper.setMax(Eigen::Vector4f(stCuboid.maxX, stCuboid.maxY, stCuboid.maxZ, 1.0));
    clipper.setInputCloud(pCloud);
    clipper.filter(*pCloudOut);
    clipper.setNegative(false); //default value: false

#else
    for(UINT32 i = 0; i < pCloud->size(); ++i) {
        if (pCloud->points[i].x < stConditionFilterCfg.minX || pCloud->points[i].x > stConditionFilterCfg.maxX ||
            pCloud->points[i].y < stConditionFilterCfg.minY || pCloud->points[i].y > stConditionFilterCfg.maxY ||
            pCloud->points[i].z < stConditionFilterCfg.minZ || pCloud->points[i].z > stConditionFilterCfg.maxZ) {
            continue;
        } else {
            pCloudOut->points.push_back(pCloud->points[i]);
        }
    }
#endif

    *pCloud = *pCloudOut;
}

/**************************************************************************************
功能描述: 半径滤波
修改记录:
**************************************************************************************/
void POINT_CLOUD_FILTER_C::RadiusOutlierFilterByPcl(MyCloud::Ptr pCloud, float searchRadius, INT32 minNeighbors)
{
    if (0 == pCloud->size())
        return;
        
    MyCloud::Ptr pCloudOut(new MyCloud);
    pcl::RadiusOutlierRemoval<MyPoint> filter;
    
    filter.setInputCloud(pCloud);
    filter.setRadiusSearch(searchRadius);
    filter.setMinNeighborsInRadius(minNeighbors);
    filter.setNegative(false); 
    filter.filter(*pCloudOut);
    
    *pCloud = *pCloudOut;
}

/**************************************************************************************
功能描述: 基于体素的半径滤波
修改记录:
**************************************************************************************/
void POINT_CLOUD_FILTER_C::RadiusOutlierFilterByVoxel(MyCloud::Ptr pCloud, 
                                    float searchRadius, INT32 minNeighbors,
                                    CUBOID_STRU &stCuboid, float voxelSize)
{
    if (0 == pCloud->size())
        return;

    if (pobjPcptVoxel == NULL)
        pobjPcptVoxel = new PCPT_VOXEL_C(stCuboid, voxelSize);

    if (!pobjPcptVoxel->IsInit() && pobjPcptVoxel->Init() != 0)
        return;

    pobjPcptVoxel->ResetVoxelArray();
    
    vector<VOXEL_COORDINATE_STRU> vPointVoxelCoor;
    MyCloud::Ptr pCloudTemp(new MyCloud);
    
    vPointVoxelCoor.reserve(pCloud->size());
    pCloudTemp->reserve(pCloud->size());

    for (const MyPoint &point : pCloud->points) {
        VOXEL_COORDINATE_STRU stVoxelCoor;
        if (0 == pobjPcptVoxel->AddPoint2VoxelCellWithCoor(point, stVoxelCoor)) {
            vPointVoxelCoor.push_back(stVoxelCoor);
            pCloudTemp->points.push_back(point);
        }
    }

    if (searchRadius != searchRadius_) {
        searchSteps_  = searchRadius / voxelSize + 0.5;
        searchRadius_ = searchRadius;
    }
    
    MyCloud::Ptr pCloudOut(new MyCloud);
    pCloudOut->reserve(pCloud->size());
    
    for (UINT32 i = 0; i < vPointVoxelCoor.size(); ++i)
        if (pobjPcptVoxel->HasEnoughNeigVoxelValue(vPointVoxelCoor[i], searchSteps_, minNeighbors))
            pCloudOut->points.push_back(pCloudTemp->points[i]);

    *pCloud = *pCloudOut;
}

